clc;clear;close all;
field_limits = [0 100 0 100 0 20];
r_num = 4;
dataname = 'pos_his.mat';
savepath = ['Results/StatisticalResults/', num2str(r_num),'robot/'];

load([savepath,dataname])

trialNum = 50;
robot_his = pos_his{trialNum};
robot_his = cell2mat(robot_his);
robot_int = robot_his(1:r_num,:);
robot_end = robot_his(end-r_num+1:end,:);

time = length(robot_his)/r_num; scattersize1=30;
c = ['r','g','m','c'];

%density
f = figure('Name','VideoMaker'); hold on; grid on; view(-40,20); axis(field_limits); f.Visible = 'off';
% field_limits1 = field_limits;
% discretization_num = 500; mesh_size = field_limits(2)/discretization_num;
% x_space = linspace(field_limits1(1),field_limits1(2),discretization_num);
% y_space = linspace(field_limits1(3),field_limits1(4),discretization_num);
% [X_points,Y_points] = meshgrid(x_space,y_space);
% points_xy = [reshape(X_points,discretization_num^2,1) reshape(Y_points,discretization_num^2,1)];
d_centers = [50 50];  j = 0;
set(f,'position',[1 1 860 460]); hold on;
% R = densityFunc(d_centers,points_xy);
% surf(X_points,Y_points,R);shading interp; colorbar; hold on;
%  j = 10;
% F(1:j) = getframe(f);
time = size(robot_his,1)/r_num;
for t = 1:1:time
    t/time
    j = j + 1;
    pos = robot_his(r_num*(t-1)+1:r_num*(t),:);
    [X,Y,Z] = computeMeshforPlot(pos,field_limits,200);
    pcolor(X,Y,Z); shading interp; colorbar;caxis manual; caxis([0 1]);
    for i = 1:r_num
        scatter3(pos(i,1),pos(i,2),abs(pos(i,3)),scattersize1,'o',c(i),'filled');
    end
    scatter3(d_centers(1),d_centers(2),0,4*scattersize1,'^','m','filled');
    F(j) = getframe(f);
    clf(f); hold on; grid on; view(-40,20); axis(field_limits);
end
save F_raw_singlecase.mat 'F';
clc;
writerObj = VideoWriter('SingleDensitycase.avi');
writerObj.FrameRate = 10;
open(writerObj);

writeVideo(writerObj, F)
close(writerObj);
clear F;
%% trajectories from different views
rang = cell(r_num,1);
rang{1}= 1:r_num:length(robot_his);
rang{2}= 2:r_num:length(robot_his);
rang{3}= 3:r_num:length(robot_his);
rang{4}= 4:r_num:length(robot_his);
f4 = figure('Name','PositionsIn2D'); hold on; grid on;f4.Visible = 'off';
scattersize1 = 30; scattersize2 = 60; c = ['r','g','m','c'];
for i = 1:r_num
    scatter3(robot_int(i,1),robot_int(i,2),abs(robot_int(i,3)),scattersize1,'o',c(i),'filled');
    scatter3(robot_end(i,1),robot_end(i,2),abs(robot_end(i,3)),scattersize2,'v',c(i),'filled');
    scatter3(robot_his(rang{i},1),robot_his(rang{i},2),abs(robot_his(rang{i},3)),1,'.',c(i));
end
axis(field_limits);  j = 0;
for i = 1:360
    j = j+1
    view(79+i,49);
    T(j) = getframe(f4);
end
save T_diffviews_singlecase.mat 'T';
writerObj1 = VideoWriter('TrajecotriesFromDiffView.avi');
writerObj1.FrameRate = 10;
open(writerObj1);

writeVideo(writerObj1, T)
close(writerObj1);
clear T;
